/*
turners.c
offer turners turn a corner function
designed by lunanting

--------------------------
2020-05-11 20:53:39
create file
--------------------------
*/
#include "turners.h"
#include "string.h"
#include "stdio.h"
#include "stdlib.h"
#include "aos/kv.h"


static TURNERS_class turners={0};

static u8 move_quit=0;
/*
turners object init
转向器初始化
param:
    port:modbus uart port
    speed:modbus uart speed
    slave_id:slave device address
return:
    0=success
*/
int xp_turners_init(u8 port,u32 speed,u8 slave_id){
    char v[20]={0};
    int v_len=20;

    turners.motor.port=port;
    turners.motor.speed=speed;
    turners.motor.slave_id=slave_id;
    xp_turners_config();
    if(xp_pans_init(&turners.motor)!=0){
        println("error:turners init fail~");
        return -1;
    }
    if(xp_turners_get_encode()!=0)return -2;
    aos_kv_get("turners.origin",v,&v_len);
    if(strlen(v)>0){
        println("turners use saved origin data,%s~",v);
        turners.origin_encode=atoi(v);
    }
    else{
        println("turners use location as origin ,now~");
        turners.origin_encode=turners.encode;
    }
    aos_msleep(10);
    xp_turners_get_location();
    turners.init=1;
    println("turners init success~,config:port=%d speed=%d id=%d",port,speed,slave_id);
    return 0;
}

int xp_turners_recover(u8 port,u32 speed,u8 slave_id){
    char v[20]={0};
    int v_len=20;

    turners.motor.port=port;
    turners.motor.speed=speed;
    turners.motor.slave_id=slave_id;
    xp_turners_config();
    if(xp_pans_init(&turners.motor)!=0){
        println("error:turners init fail~");
        return -1;
    }
    xp_turners_get_location();

    println("turners recover success~,config:port=%d speed=%d id=%d",port,speed,slave_id);
    return 0;
}

/*
config turners motor
*/
void xp_turners_config(void){
    int motor_speed=xp_turners_convert_speed(TURNERS_angle_speed);
    PANS_handler *motor=&turners.motor;
    //限位配置
    motor->config.home_offset=0;
    motor->config.max_limit=1000000;
    motor->config.min_limit=-1000000;

    //配置电机速度
    motor->config.speed[0]=100;
    motor->config.speed[1]=motor_speed;

    //配置电机加速度
    motor->config.acc[0]=10000;
    motor->config.acc[1]=10000;

    //配置电机减速度
    motor->config.dec[0]=2000;
    motor->config.dec[1]=2000;
}


/*
change angle speed to motor speed
将转向器的角速度转换成电机转速
param:
    angle_speed:turners angle speed, '/s
return:motor speed ,r/min
*/
int xp_turners_convert_speed(u8 angle_speed){
    return angle_speed*TURNERS_ratio/6;
}

/*
change angle to pulse
将转动角度转换成电机脉冲数
param:
    angle: turners turn' angle
return:motor pulse
*/
int xp_turners_convert_angle(s8 angle){
    return angle*TURNERS_part*TURNERS_ratio/360;
}

/*
clear turners warn
*/
int xp_clear_turners_warn(void){
    PANS_handler *motor = &turners.motor;
    gpio_dev_t gpio;
	uint32_t turner_err = 0;
    u8 clear_warn_try_again_cnt;

    //检测是否清成功，不成功重试
    clear_warn_try_again_cnt = 5;
    while(clear_warn_try_again_cnt--)
    {
        aos_msleep(50);
        if(xp_pans_en(motor,pans_off)!=0)
        {
            println("turners enable off faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(xp_pans_run(motor,pans_off)!=0)
        {
            println("turners stop config faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(xp_pans_clear_warn(motor)!=0)
        {
            println("turners warn clear faild!!!!!!!!");
            continue;
        }
        aos_msleep(50);
        if(xp_pans_en(motor,pans_on)!=0)
        {
            println("turners enable on faild!!!!!!!!");
            continue;
        }

        //举平驱动器错误检测
        gpio.port = 109;
        hal_gpio_input_get(&gpio, &turner_err);
        if(1 == turner_err)
        {
            aos_msleep(200);
        }
        else
        {
            xp_turners_get_location();//更新机构的location
            println("turnner warn clear success! try cnt = %d",(5-clear_warn_try_again_cnt));
            xp_io_write_pin(1,14,0);                //松开举平抱闸
            return 0;
        }
    }

    println("turners warn clear faild!!!");
    return -1;
}

/*
turners move support retry
param:
    angle: move angle
    speed_block:speed block
    retry_cnt：want retry num
return:
    0=success
*/
int xp_turners_move_support_retry(s8 angle,u8 speed_block,u8 retry_cnt){
    int ret = -1;
    u8 retry_num = 0;

    retry_num = retry_cnt;
    ret = xp_turners_move(angle,speed_block);
    while(retry_num--){
        if(ret != 0){
            aos_msleep(200);
            println("turners move failed, retry num %d", retry_num);
            ret = xp_turners_move(angle,speed_block);
        }
        else{
            return ret;
        }
    }
    if(ret != 0){
        println("turners move failed, retry cnt %d", retry_cnt);
    }
    return ret;
}

/*
turners move
param:
    angle: move angle
    speed_block:speed block
return:
    0=success
*/
int xp_turners_move(s8 angle,u8 speed_block){
    int pulse=0;
    s8 H=0;
    PANS_handler *motor=&turners.motor;
    u8 count=0;
    int sta=0;
    u8 flag=0,abnormal=0;
    u64 time_last=0,time_need=0;
    int ret = -1;

    if(turners.init!=1)return -1;
TYR:if(turners.run==1)return -2;
	//xp_io_write_pin(1,14,0);
    turners.run=1;
    turners.angle=angle;
    move_quit=0;
    H=angle-turners.location;
    if(H==0)goto END;
    pulse=xp_turners_convert_angle(H);
    if(xp_turners_check_warn()!=0){
        //举平有报警时，先清报警
        ret = xp_clear_turners_warn();
        if(ret != 0){
            sta= -3;goto END;
        }
        aos_msleep(100);
    }
    // if(angle==0){
    //     println("turners motor slow stop!");
    //     if(xp_pans_slow_stop(motor,pans_on)!=0){sta= -4;goto END;}
    //     aos_msleep(500);
    // }
    // else{
        if(xp_pans_get_buzy_state(motor)==pans_on){ //正在运行
            println("turners motor slow stop first!");
            if(xp_pans_slow_stop(motor,pans_on)!=0){sta= -5;goto END;}
            aos_msleep(500);
            if(xp_pans_slow_stop(motor,pans_off)!=0){sta= -6;goto END;}
        }
        println("turners motor  move a angle=%d,pluse=%d",angle,pulse);
        if(xp_pans_move_s(motor,speed_block,pulse)!=0){sta= -7;goto END;}
        turners.location=angle;
        time_need=(abs(H))*1000/10+1000;
        time_last=aos_now_ms();
        while(xp_pans_get_buzy_state(motor)==pans_on){
            count++;
            if(pulse>0){
                if(xp_io_read_input_pin(1,TURNERS_io_top)==TURNERS_io_touch){
                    xp_pans_slow_stop(motor,pans_on);
                    aos_msleep(500);
                    xp_pans_slow_stop(motor,pans_off);
                    turners.location=90;
                    println("turner stop ,by top limit io !");
                    abnormal=1;
                    break;
                }
            }
            else if(pulse<0){
                if(xp_io_read_input_pin(1,TURNERS_io_down)==TURNERS_io_touch){
                    xp_pans_slow_stop(motor,pans_on);
                    aos_msleep(500);
                    xp_pans_slow_stop(motor,pans_off);
                    turners.location=-10;
                    println("turner stop ,by bottom limit io !");
                    abnormal=1;
                    break;
                }
            }
            // else if(angle==0){
            //     if(xp_io_read_input_pin(1,TURNERS_io_origin)==TURNERS_io_touch){
            //         xp_pans_slow_stop(motor,pans_on);
            //         aos_msleep(500);
            //         xp_pans_slow_stop(motor,pans_off);
            //         turners.location=0;
            //         break;
            //     }
            // }
            if(xp_turners_check_warn()!=0){
                println("turner stop ,by turner error !");
                sta=-9;
                abnormal=1;
                break;
            }
            if(count>150){
                xp_pans_slow_stop(motor,pans_on);
                aos_msleep(500);
                xp_pans_slow_stop(motor,pans_off);
                println("turner stop ,by over time !");
                sta=-8;
                abnormal=1;
                break;
            }

            if((aos_now_ms()-time_last)>time_need){
                xp_pans_slow_stop(motor,pans_on);
                aos_msleep(500);
                xp_pans_slow_stop(motor,pans_off);
                println("turner move over time !");
                sta=-11;
                abnormal=1;
                break;
            }
            if(move_quit==1){
                abnormal=1;
                break;
            }
            aos_msleep(100);
        }
    // }
    if(xp_turners_check_warn() != 0){
        sta=-9;
        abnormal=1;
    }
END:turners.run=0;
    // turners.location+=angle;
	//xp_io_write_pin(1,14,1);
    if(abnormal!=0){
        xp_turners_get_location();
        if(turners.location!=angle){
            println("turner get location (%d) != angle (%d),try again !",turners.location,angle);
            xp_turners_get_location();
        }
    }
    if(sta!=0){
        perr("turner move fail ~ return code = %d !",sta);
    }
    return sta;
}

/*
turners check zero
转向器校零
return:
    0=success
*/
int xp_turners_check_zero(void){
    PANS_encode encode;

    turners.check_zero=0;
    if(xp_io_read_input_pin(1,TURNERS_io_top)!=TURNERS_io_touch){
        if(xp_turners_move(15,1)!=0)return -1;
    }
    turners.location=90;
    if(xp_turners_move(0,1)!=0)return -2;
    if(xp_io_read_input_pin(1,TURNERS_io_origin)==TURNERS_io_touch){
        turners.check_zero=1;
        turners.location=0;
        xp_pans_clear_location(&turners.motor);
        if(xp_pans_get_encode(&turners.motor,&encode)!=0)return -3;
        turners.encode=encode.h;
        turners.origin_encode=turners.encode;
        println("turners motor check zero success, encode value=%.2f",turners.encode);
        return 0;
    }
    else{
        println("turners motor check zero fail~");
        return -5;
    }
}


/*
turners check warn and repair
return:
    0=success
*/
int xp_turners_check_warn(void){
    gpio_dev_t gpio;
    u32 value=0;
    gpio.port = 109;
    hal_gpio_input_get(&gpio, &value);
    if(value == 1){
        perr("turner motor have a error !!");
        xp_pans_get_error_code(&turners.motor);
        return -1;
        // if(xp_pans_clear_warn(&turners.motor)!=0)return -1;
        // if(xp_pans_en(&turners.motor,pans_on)!=0)return -2;
    }
    return 0;
}

/*
turner reset angle
param:
	angle:set angle
return:
	0=success
*/
int xp_turners_set_angle(int angle){ 
	turners.location=angle;
}


/*
get turners motor encode
return:
    0=success
*/
int xp_turners_get_encode(void){
    PANS_encode encode={0};

    if(xp_pans_get_encode(&turners.motor,&encode)!=0){
        perr("get turners encode fail~");
        return -1;
    }
    println("turner encode=%d.%d    %d",encode.r,encode.p,encode.h);
    turners.encode=encode.h;
    return 0;
}


/*
get turners location base on encode
return:
    0=success
*/
int xp_turners_get_location(void){
    int d=0;

    if(xp_turners_get_encode()!=0)return turners.location;
    d=turners.encode-turners.origin_encode;
    turners.location=((d/838.8608)/TURNERS_part)*360.0/TURNERS_ratio;
    println("turners encode %d,origin= %d,location=%d",turners.encode,turners.origin_encode,turners.location);
    return turners.location;
}

/*
get turners location base on encode,but dont updata turners.location
return:
    real_time_location
//在举平运动过程中获取当前的实时位置，不更新turners.location的值，该值为运动的结束值
*/
int xp_turners_get_location_dont_update_value(void){
    int d=0;
    s8 turners_real_time_location = 0;

    if(xp_turners_get_encode()!=0)return -10;
    d=turners.encode-turners.origin_encode;
    turners_real_time_location=((d/838.8608)/TURNERS_part)*360.0/TURNERS_ratio;
    println("turners encode %d,origin= %d,real_time_location=%d",turners.encode,turners.origin_encode,turners_real_time_location);
    return turners_real_time_location;
}


/*
set as zero point
return:
    0=success
*/
int xp_turners_as_zero(void){
    char v[20];

    if(xp_turners_get_encode()!=0)return -1;
    turners.origin_encode=turners.encode;
    turners.location=0;
    sprintf(v,"%d",turners.origin_encode);
    if(aos_kv_set("turners.origin",v,strlen(v),1)!=0)return -2;
    println("turners.origin set as: %s",v);
}


/*
clear zero recode
*/
void xp_turners_zero_clear(void){
    aos_kv_del("turners.origin");
}


/*
get tuern angle
return: angle value ,degree 
*/
int xp_turner_angle_get(void){
    return turners.location;
}


/*
turner move a angle
param:
    angle: move to the degree
return:
    0=success
*/
int xp_turner_move_to_angle(int angle){
    return xp_turners_move(angle,1);
}


/***********************io deal with**********************************/
/*
turners io touch event deal with
*/
void xp_turners_io_event(u32 event){
    static u8 down=1,origin=1,top=1;
    u8 state=0,stop_flag=0;

    state=(event>>TURNERS_io_down)&0x01;
    if(state!=down){
        down=state;
        if(down==TURNERS_io_touch&&turners.angle>0&&turners.run==1){
            stop_flag=1;
            println("turners down io touched ! slow stop motor~");
        }
    }
    state=(event>>TURNERS_io_origin)&0x01;
    if(state!=origin){
        origin=state;
        if(origin==TURNERS_io_touch&&turners.check_zero==0&&turners.run==1){
            stop_flag=1;
            println("turners origin io touched ! slow stop motor~");
        }
    }
    state=(event>>TURNERS_io_top)&0x01;
    if(state!=top){
        top=state;
        if(top==TURNERS_io_touch&&turners.angle<0&&turners.run==1){
            stop_flag=1;
            println("turners top io touched ! slow stop motor~");
        }
    }

    if(stop_flag==1){
        stop_flag=0;
        move_quit=1;
        xp_pans_slow_stop(&turners.motor,pans_on);
        aos_msleep(500);
        xp_pans_slow_stop(&turners.motor,pans_off);
    }
}


/*****************************turners debug ***************************/
/*
debug
*/
int xp_turners_debug(char *type,char *fun,char *param){
    s16 angle=0;

    if(strcmp(type,"turners")!=0)return 0;
    if(strcmp(fun,"check_zero")==0){
        if(xp_turners_check_zero()!=0){
            println("turners check zero success!");
        }
        else{
            println("turners check zero fail~");
        }
    }
    else if(strcmp(fun,"support_retry")==0){
        angle=atoi(param);
        if(angle<100&&angle>-100){
            xp_turners_move_support_retry(angle,1,3);
        }
        else{
            println("error:turners move angle over legal area,angle=%d",angle);
        }
    }
    else if(strcmp(fun,"move")==0){
        angle=atoi(param);
        if(angle<100&&angle>-100){
            xp_turners_move(angle,1);
        }
        else{
            println("error:turners move angle over legal area,angle=%d",angle);
        }
    }
    else if(strcmp(fun,"read")==0){
        if(strcmp(param,"location")==0){
            println("turners location=%d",turners.location);
        }
    }
    else if(strcmp(fun,"block")==0){
        if(xp_pans_get_buzy_state(&turners.motor)==pans_on){
            xp_pans_slow_stop(&turners.motor,pans_on);
            aos_msleep(500);
            xp_pans_slow_stop(&turners.motor,pans_off);
        }
        else{
            xp_pans_clear_warn(&turners.motor);
            xp_pans_en(&turners.motor,pans_on);
            aos_msleep(500);
        }
        xp_pans_run(&turners.motor,pans_off);
        xp_pans_set_block(&turners.motor,atoi(param));
        xp_pans_run(&turners.motor,pans_on);
    }
    else if(strcmp(fun,"location")==0){
        xp_turners_get_location();
    }
    else if(strcmp(fun,"set_zero")==0){
        xp_turners_as_zero();
    }
    else if(strcmp(fun,"clear_zero")==0){
        xp_turners_zero_clear();
    }
    else{
        return 0;
    }

    return 1;
}
//end of the file

